#pragma once

#include "cartographer/rcm/imu/preintegrate.h"

namespace cartographer
{
    class PoseIntegrator
    {
    public:
        using Ptr = std::shared_ptr<PoseIntegrator>;

        PoseIntegrator();

        int Integrate(const std::vector<sensor::ImuData> imuSet,
                      const common::Time &time_start,
                      const common::Time &time_end);

        void Predict(const TimedPose &lastPose, const common::Time &currTime, TimedPose &currPose);

        ImuPreintegrate::Ptr integrator_{};

        ImuBias last_bias_;

        Eigen::Vector3d linear_velocity_ = Eigen::Vector3d::Zero();
    };
} // namespace cartographer